package com.rockwell.maze800.model;

import java.util.ArrayDeque;
import java.util.Deque;

import net.wimpi.modbus.ModbusException;

import org.opencv.core.Point;

import android.annotation.SuppressLint;
import android.os.Handler;
import android.util.Log;
import android.widget.Toast;


enum PickEnum 
{
	START,
	STOP_ROBOT,
	MOVE_BACK_FOR_MISSING_TREASURE,
	GET_TREASURE_POSITION,
	INDEX_TO_HORIZONTAL_PICK_POSITION,
	MOVE_BACK_BEFORE_ARM_DOWN,
	ARM_DOWN,
	MOVE_FORWARD_TO_DETECT_TREASURE,
	CHECK_FOR_TREASURE_BOX,
	JOG_TO_VERTICAL_PICK_POSITION,
	STOP_JOG_TO_VERTICAL_PICK_POSITION,
	CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK,
	INDEX_TO_VERTICAL_PICK_POSITION,
	FINAL_INDEX_TO_VERTICAL_PICK_POSITION,
	PICK,
	MOVE_BACK_TO_ENSURE_PICK_AREA_WITHIN_FOV,
	VERIFY_TREASURE_PICKED,
	ENSURE_ARM_UP_AFTER_PICK,
	CHECK_DRIFT_FROM_INITIAL_ORIENTATION,
	TURN_TO_INITIAL_ORIENTATION_BEFORE_U_TURN,
	JOG_TO_DEAD_END,
	WAIT_JOG_TO_DEAD_END_DONE,
	MOVE_BACK_AT_DEAD_END,
	U_TURN_AFTER_PICK,
	DONE,
	ERROR;
}

enum JogEnum 
{
	START,
	DETECT_BARRIER,
	DETECT_TRASH,
	DETECT_TREASURE,
	ENSURE_ARM_UP,
	COMPASS_COMPENSATION,
	UPDATE_NEXT_ACTION,
	START_JOG,
	WAIT,
	ADJUST,
	DETECT_FRONT_WALL,
	DONE,
	ERROR;
}

enum TurnEnum 
{
	START,
	STOP_JOG,
	MOVE_BACK_AT_DEAD_END,
	ENSURE_ARM_UP,
	CHECK_TO_ENSURE_FRONT_SENSOR_OFF,
	MOVE_AFTER_FRONT_SENSOR_OFF,
	MOVE_UNTIL_FRONT_SENSOR_OFF,
	DETERMINE_AMOUNT_TO_MOVE_WITHOUT_FRONT_WALL,
	MOVE_WITHOUT_FRONT_WALL,
	CAPTURE_ORIENTATION_BEFORE_TURN,
	DETERMINE_U_TURN_DIRECTION,
	TURN,
	CHECK_ORIENTATION,
	COMPENSATE_TURN,
	MOVE_OUT_OF_JUNCTION,
	POST_MOVE_OUT_OF_JUNCTION,
	DONE,
	ERROR;
}

enum TurnBackEnum 
{
	START,
	STOP_JOG,
	ENSURE_ARM_UP,
	CHECK_IF_JUCTION_REACHED,
	REVERSE_UNTIL_JUNCTION_REACHED,
	REVERSE_INTO_JUNCTION,
	CAPTURE_ORIENTATION_BEFORE_TURN,
	TURN,
	CHECK_ORIENTATION,
	COMPENSATE_TURN,
	CHECK_IF_OUT_OF_JUCNTION,
	MOVE_OUT_OF_JUNCTION,
	POST_MOVE_OUT_OF_JUNCTION,
	DONE,
	ERROR;
}

enum CompassCompensationEnum 
{
	START,
	DETERMINE_ADJUST_TURN_ANGLE,
	STOP_ROBOT,
	ADJUST,
	RESUME_CMD,
	DONE,
	ERROR;
}

enum AdjustEnum 
{
	START,
	DETERMINE_ADJUST_TURN_ANGLE,
	STOP_JOG,
	ADJUST,
	START_JOG,
	DONE,
	ERROR;
}

enum SendCommandEnum
{
	SEND,
	WAIT,
	ERROR
}

enum SendJog2Enum
{
	SEND,
	WAIT,
	DETECT_BARRIER,
	COMPASS_COMPENSATION,
	ERROR
}

enum ArmUpEnum
{
	START,
	STOP_JOG,
	CHECK_UP,
	REVERSE,
	MOVE_UP,
	FORWARD,
	START_JOG,
	ERROR
}

enum DetectFrontWallEnum
{
	START,
	STOP_JOG,
	ADJUST_BACK,
	ENSURE_ARM_UP,
	MOVE_FORWARD_SIDE_SENSOR_OFFSET_DIST,
	INDEX_TO_DETECT_FRONT_WALL,
	UPDATE_NEXT_ACTION,
	CONTINUE_JOGGING,
	POST_MOVE_FOR_CONTINUE_JOGGING,
	ERROR
}

enum ClearTrashEnum
{
	START,
	STOP_ROBOT,
	JOG_TO_VERTICAL_RETURN_POSITION,
	CHECK_FOR_TRASH_BOX,
	STOP_JOG_TO_RETURN_POSITION,
	CHECK_FOR_TRASH_ABSENCE_BEFORE_RETURN,
	INDEX_TO_RETURN_POSITION,
	U_TURN,
	START_JOG,
	ERROR,
}

enum ClearBarrierEnum
{
	START,
	STOP_ROBOT,
	WAIT,
	RESUME_CMD,
	ERROR
}

enum RobotState 
{
	IDLE,
	INITIALIZED,
	RUNNING,
	PAUSING,
	PAUSED,
}

@SuppressLint("DefaultLocale")
public class Robot {
	public enum RunEnum 
	{
		START,
		PERFORM_JOG,
		PERFORM_TURN_RIGHT,
		PERFORM_TURN_LEFT,
		PERFORM_TURN_BACK,
		PERFORM_PICK,
		PERFORM_ADJUST_BACK,
		ERROR;
	}
	
	static final String TAG = "robot";
	
	public static int INTERVAL = 50;
	public static int CMD_TIME_OUT = 30000; // Milliseconds
	public static int PICK_TIME_OUT = 120000; // Milliseconds
	public static int MOTION_SETTLING_TIME = 0; // Milliseconds
	public static int MOTION_SETTLING_TIME2 = 3000; // Milliseconds
	public static int LANE_WIDTH = 300; // mm
	public static int FRONT_SENSOR_THRESHOLD = 100; // mm
	public static int DETECT_FRONT_WALL_INDEX = 10; // mm  << setting page
	public static int FINAL_DETECT_FRONT_WALL_INDEX = 5; // mm  << setting page
	public static int LEFT_MOVEBACKWARD_WITHOUT_FRONT_WALL = 70; // mm  << setting page
	public static int RIGHT_MOVEBACKWARD_WITHOUT_FRONT_WALL = 50; // mm  << setting page
	public static int MOVE_INDEX = 25; // mm
	public static int MOVE_BACK_TO_ENSURE_PICK_AREA_WITHIN_FOV_DIST = 40;//mm
	public static int PICK_TREASURE_FINAL_MOVE = 10; // mm
	public static int UTURN_MOVE_INDEX = 20; //mm  << setting page
	public static int TURN_INDEX = 3; // Degree
	public static int SMALL_TURN_INDEX = 2; // Degree
	public static int SMALL_MOVE_INDEX = 10; // mm
	public static int MOVE_BACK_BEFORE_ARM_DOWN_DIST = 40; // mm
	public static int DETECT_TREASURE_ABSCENT_MOVE_INDEX = 30; // mm
	public static int SIDE_SENSOR_OFFSET = 80; // mm
	public static int DETECT_FRONT_WALL_DISTANCE = 400; // mm
	public static int LEFT_REVERSE_INTO_JUNCTION = 100; // mm
	public static int MAX_PICK_TREASURE_RETRY = 3;
	public static int RIGHT_REVERSE_INTO_JUNCTION = 80; // mm
	public static int ADJUST_ANGLE = 0; // Degree
	public static int COMPENSATION_ANGLE_TOLERANCE = 5; // Degree
	
	private SensorID m_sensorDetectedOpening = SensorID.Max;
	private int m_cmdTimeOut = 0;
	private int m_compensationAngle = 0;
	private int m_pickTreasureRetryCount = 0;
	private RobotPosition m_robotPosition;
	private Point m_treasureDrift;
	private Point m_trashDrift;
	private int m_adjustAngle = 0;
	private int m_amountToAdjust = 0;
	private int m_amountToIndex = 0;
	private int m_idealOrientation = 0;
	private int m_curOrientation = 0;
	private int m_turn_direction = 1;
	private int m_turn_angle = 0;
	private int m_turn_count = 1;
	private int m_logLevel = 1;
	Deque<RunEnum> actionHistory = new ArrayDeque<RunEnum>(128);
	private Object m_pauseResumeLock = new Object();
	private boolean stepMode = false;
	private boolean useCompassCompensation = false;
	private boolean useCompassCompensationDuringTurn = false;
	private RobotState m_robotState = RobotState.IDLE;
	private boolean m_needSendJogCommandBeforeStart = false;

	
	private RunEnum mainSequence = RunEnum.START;
	private PickEnum pickSequence = PickEnum.START;
	private JogEnum jogSequence = JogEnum.START;
	private TurnEnum turnSequence = TurnEnum.START;
	private TurnEnum turnBackSequence = TurnEnum.START;
	private AdjustEnum adjustSequence = AdjustEnum.START;
	private CompassCompensationEnum compassCompensationSequence = CompassCompensationEnum.START;
	private AdjustEnum adjustBackSequence = AdjustEnum.START;
	private SendCommandEnum sendCommandSequence = SendCommandEnum.SEND;
	private SendJog2Enum sendJog2Sequence = SendJog2Enum.SEND;
	private ArmUpEnum armUpSequence = ArmUpEnum.START;
	private DetectFrontWallEnum detectFrontWallSequence = DetectFrontWallEnum.START;
	private ClearBarrierEnum clearBarrierSequence = ClearBarrierEnum.START;
	private ClearTrashEnum clearTrashSequence = ClearTrashEnum.START;
	
	private RunEnum mainRecoverSequence = RunEnum.START;
	private PickEnum pickRecoverSequence = PickEnum.START;
	private JogEnum jogRecoverSequence = JogEnum.START;
	private TurnEnum turnRecoverSequence = TurnEnum.START;
	private TurnEnum turnBackRecoverSequence = TurnEnum.START;
	private CompassCompensationEnum compassCompensationRecoverSequence = CompassCompensationEnum.START;
	private AdjustEnum adjustRecoverSequence = AdjustEnum.START;
	private AdjustEnum adjustBackRecoverSequence = AdjustEnum.START;
	private SendCommandEnum sendCommandRecoverSequence = SendCommandEnum.SEND;
	private SendJog2Enum sendJog2RecoverSequence = SendJog2Enum.SEND;
	private ArmUpEnum armUpRecoverSequence = ArmUpEnum.START;
	private DetectFrontWallEnum detectFrontRecoverWallSequence = DetectFrontWallEnum.START;
	private ClearBarrierEnum clearBarrierRecoverSequence = ClearBarrierEnum.START;
	private ClearTrashEnum clearTrashRecoverSequence = ClearTrashEnum.START;
	
	private double m_robotWidth = 15; // 15cm
	private double m_robotLength = 15; // 15cm
	
	private boolean m_gameOver = false;
	
	private Handler m_missionHandler = new Handler();
	private Runnable m_mission = new Runnable() {

		@Override
		public void run() {
			log("Robot Started", 1);
			while(true) {
				try {
					if (m_robotState == RobotState.IDLE) {
						initialize();
					}
					
					if (m_robotState == RobotState.INITIALIZED || 
						m_robotState == RobotState.PAUSING) {
						runBeforePause();
						waitForResume();
						runBeforeStart();
					}
					
					continueMission();
				}
				catch (Exception ex) {
					log(ex, 1);
					runBeforePause();
					waitForResume();
					runBeforeStart();
				}
			}
		}
		
	};

	private Thread m_backgroundThread = new Thread(m_mission);
	
	private IRobotSensors m_sensors;
	private IRobotCommander m_commander;
	private Maze m_maze;
	private IMazeSolver m_solver;
	private IRobotPositionDetector m_positionDetector;
	private IDetector m_treasureDetector;
	private IDetector m_barrierDetector;
	private IDetector m_trashDetector;
	
	// avoid creating lots of RobotCommand objects
	private RobotCommand m_command = new RobotCommand();
	
	private IRobotLogger m_logger;
	
	public Robot(double width, double length, IRobotSensors sensors, IRobotCommander commander, Maze maze, IMazeSolver solver, 
			IDetector treasureDetector, 
			IDetector barrierDetector,
			IDetector trashDetector) {
		m_sensors = sensors;
		m_positionDetector = new RobotPositionDetector(this, maze, sensors, commander);
		m_treasureDetector = treasureDetector;
		m_barrierDetector = barrierDetector;
		m_trashDetector = trashDetector;
		m_commander = commander;
		m_maze = maze;
		m_solver = solver;
	}

	public void setLogger(IRobotLogger logger, int level) {
		m_logger = logger;
		m_logLevel = level;
	}
	
	public void log(String msg, int level) {
		if (m_logger != null && m_logLevel >= level) {
			m_logger.log(msg);
			
			if ((stepMode && level <= 2) || level == 0) {
				runBeforePause();
				waitForResume();
				runBeforeStart();
			}
		}
	}
	
	private void logSensors() {
		String msg;
		msg = String.format("F[%s],L[%s],R[%s]", m_robotPosition.frontwalldetected? "Y":"N",
								m_robotPosition.leftwalldetected? "Y":"N",
								m_robotPosition.rightwalldetected? "Y":"N");
		log(msg, 2);
	}
	
	public void log(Exception ex, int level) {
		if (m_logger != null && m_logLevel >= level) {
			m_logger.log(ex);
		}
	}

	public void changeSolver(IMazeSolver solver) {
		m_solver = solver;
	}
	
	public double getWidth() {
		return m_robotWidth;
	}
	
	public void setWidth(double width) {
		m_robotWidth = width;
	}
	
	public double getLength() {
		return m_robotLength;
	}
	
	public void setLength(double length) {
		m_robotLength = length;
	}
	
	public void setOrientation(float angle) {
		m_sensors.setOrientation(angle);
	}
	
	public void setTreasure(boolean present, Point center, float radius, int length, int width) {
		m_treasureDetector.onDetect(present, center, radius, length, width);
	}
	
	public void setBarrier(boolean present, Point center, float radius, int length, int width) {
		m_barrierDetector.onDetect(present, center, radius, length, width);
	}
	
	public void setTrash(boolean present, Point center, float radius, int length, int width) {
		m_trashDetector.onDetect(present, center, radius, length, width);
	}

	public void useStepMode(boolean use) {
		this.stepMode = use;
	}
	
	/**
	 * The location of the sensor from the left edge of the robot.
	 * @return
	 */
	public double getLeftDistanceSensorLocation() {
		return 2.5;	
	}
	
	/**
	 * The location of the sensor from the front of the robot.
	 * @return
	 */
	public double getFronDistanceSensorLocation() {
		return 1.5;
	}
	
	public void setStepMode(boolean step) {
		stepMode = step;
	}
	
	public void setUseCompassCompensation(boolean use) {
		useCompassCompensation = use;
	}
	
	public void setUseCompassCompensationDuringTurn(boolean use) {
		useCompassCompensationDuringTurn = use;
	}
	
	public void start(boolean startThread) throws Exception {
		m_gameOver = false;

		m_commander.connect();
		
		if (m_commander.isConnected()) {
			
			if (m_logger != null)
				m_logger.log("Modbus connected");
			
			//m_missionHandler.postDelayed(m_mission, INTERVAL);
			if(startThread) {
				m_backgroundThread.start();
				
				if (m_logger != null)
					m_logger.log("thread started");
			}
			
			Log.i(TAG, "started");
		}
		else {
			throw new Exception("Cannot establish Modbus connection.");
		}
	}

	public void quit() {
		m_gameOver = true;
		
		Log.i(TAG, "quit");
	}
	
	public void initialize() {
		reconnectModbus();
		
		sendCommandSequence = SendCommandEnum.SEND;
		
		while (performSendCommand(RobotAction.Jog, m_idealOrientation) != 1) {
			if (sendCommandSequence == SendCommandEnum.ERROR) {
				sendCommandSequence = SendCommandEnum.SEND;
				break;
			}
		};

		mainSequence = RunEnum.START;
		pickSequence = PickEnum.START;
		jogSequence = JogEnum.START;
		turnSequence = TurnEnum.START;
		turnBackSequence = TurnEnum.START;
		compassCompensationSequence = CompassCompensationEnum.START;
		adjustSequence = AdjustEnum.START;
		adjustBackSequence = AdjustEnum.START;
		sendCommandSequence = SendCommandEnum.SEND;
		sendJog2Sequence = SendJog2Enum.SEND;
		armUpSequence = ArmUpEnum.START;
		detectFrontWallSequence = DetectFrontWallEnum.START;
		clearBarrierSequence = ClearBarrierEnum.START;
		clearTrashSequence = ClearTrashEnum.START;
		
		mainRecoverSequence = RunEnum.START;
		pickRecoverSequence = PickEnum.START;
		jogRecoverSequence = JogEnum.START;
		turnRecoverSequence = TurnEnum.START;
		turnBackRecoverSequence = TurnEnum.START;
		compassCompensationRecoverSequence = CompassCompensationEnum.START;
		adjustRecoverSequence = AdjustEnum.START;
		adjustBackRecoverSequence = AdjustEnum.START;
		sendCommandRecoverSequence = SendCommandEnum.SEND;
		sendJog2RecoverSequence = SendJog2Enum.SEND;
		armUpRecoverSequence = ArmUpEnum.START;
		detectFrontRecoverWallSequence = DetectFrontWallEnum.START;
		clearBarrierRecoverSequence = ClearBarrierEnum.START;
		clearTrashRecoverSequence = ClearTrashEnum.START;

		sleep(2000);

		m_commander.reset();
		m_needSendJogCommandBeforeStart = false;
		m_idealOrientation = (int)m_sensors.getOrientation();

		String msg = String.format("Initial Orientation[%d] @ continueMission", m_idealOrientation);
		log(msg, 2);
		
		m_robotState = RobotState.INITIALIZED;
	}
	
	public void restart() {
		synchronized (m_pauseResumeLock) {
			m_robotState = RobotState.IDLE;
			m_pauseResumeLock.notify();
		}
	}
	
	public boolean isPaused() {
		return (m_robotState == RobotState.PAUSED);
	}
	
	public void resumeRun() {
		synchronized (m_pauseResumeLock) {
			m_pauseResumeLock.notify();
	  	}
	}
	
	public void pause() {
		synchronized (m_pauseResumeLock) {
			if (m_robotState != RobotState.PAUSED)
				m_robotState = RobotState.PAUSING;
	  	}
	}
	
	private void waitForResume() {

		synchronized (m_pauseResumeLock) {
			try {
				if (m_robotState == RobotState.IDLE)
					return;
				
				m_robotState = RobotState.PAUSED;
				if (m_logger != null)
					m_logger.onRobotPaused();
				
				m_pauseResumeLock.wait();
				
				if (m_robotState != RobotState.IDLE) {
					m_robotState = RobotState.RUNNING;
					if (m_logger != null)
						m_logger.onRobotResumed();
				}
				else {
					if (m_logger != null)
						m_logger.onRobotIdle();
				}

			}
			catch(InterruptedException e) { 
				Log.i(TAG, "resume error"); 
			} 
		}
	}
	
	private void runBeforeStart() {
		sendCommandSequence = SendCommandEnum.SEND;
		
		if (m_needSendJogCommandBeforeStart) {
			while (performSendCommand(RobotAction.Jog, m_idealOrientation) != 1) {
				if (sendCommandSequence == SendCommandEnum.ERROR) {
					sendCommandSequence = SendCommandEnum.SEND;
					break;
				}
			};
			
			m_needSendJogCommandBeforeStart = false;
		}
	}
	
	private void runBeforePause() {
		sendCommandSequence = SendCommandEnum.SEND;
		
		if (m_commander.isJogging()) {
			while (performSendCommand(RobotAction.Stop) != 1) {
				if (sendCommandSequence == SendCommandEnum.ERROR) {
					sendCommandSequence = SendCommandEnum.SEND;
					break;
				}
			};
			m_needSendJogCommandBeforeStart = true;
		}
	}

	private void continueMission() {
		switch (mainSequence)
		{
		case START:
		{
			/*
			 * Initialize variables here.
			 */
			jogSequence = JogEnum.START;
			mainSequence = RunEnum.PERFORM_JOG;
		}
		case PERFORM_JOG:
			performJog();
			
			if (mainSequence != RunEnum.PERFORM_JOG && m_adjustAngle != 0) {
				mainRecoverSequence = mainSequence;
				adjustBackSequence = AdjustEnum.START;
				mainSequence = RunEnum.PERFORM_ADJUST_BACK;
			}
			break;
		case PERFORM_TURN_RIGHT:
			if (performTurnRight() == 1) {
				m_solver.onJunctionPassed(RobotAction.TurnRight);
				actionHistory.add(RunEnum.PERFORM_TURN_RIGHT);
				jogSequence = JogEnum.START;
				mainSequence = RunEnum.PERFORM_JOG;
				log("Turn Right Done @ continueMission", 1);
			}
			break;
		case PERFORM_TURN_LEFT:
			if (performTurnLeft() == 1) {
				m_solver.onJunctionPassed(RobotAction.TurnLeft);
				actionHistory.add(RunEnum.PERFORM_TURN_LEFT);
				jogSequence = JogEnum.START;
				mainSequence = RunEnum.PERFORM_JOG;
				log("Turn Left Done @ continueMission", 1);
			}
			break;
		case PERFORM_TURN_BACK:
			if (performTurnBack() == 1) {
				actionHistory.add(RunEnum.PERFORM_TURN_BACK);
				jogSequence = JogEnum.START;
				mainSequence = RunEnum.PERFORM_JOG;
				log("Turn Back Done @ continueMission", 1);
			}
			break;
		case PERFORM_PICK:
			if (performPick() == 1) {
				actionHistory.add(RunEnum.PERFORM_PICK);
				jogSequence = JogEnum.START;
				mainSequence = RunEnum.PERFORM_JOG;	
				log("Pick Treasure Done @ continueMission", 1);
			}
			break;
		case PERFORM_ADJUST_BACK:
			if (performAdjustBack() == 1) {
				mainSequence = mainRecoverSequence;
				log("Adjust Back Done @ continueMission", 1);
			}
			break;
		case ERROR:
			break;
		default:
			mainSequence = RunEnum.START;
			break;
		}
	}
	
	private void performJog() {
		switch (jogSequence)
		{
		case START:
			/*
			 * Initialize variables here.
			 */
			sendCommandSequence = SendCommandEnum.SEND;
			jogSequence = JogEnum.START_JOG;
		case START_JOG:
			if (!m_barrierDetector.isPresent()) {

				if (performSendCommand(RobotAction.Jog, m_idealOrientation) != 1)
					break;
				
				log("Send Jog Done @ performJog", 4);
			}
			
			clearBarrierSequence = ClearBarrierEnum.START;
			jogSequence = JogEnum.DETECT_BARRIER;
		case DETECT_BARRIER:
			if (performClearBarrier(RobotAction.Jog) == 1) {
				clearTrashSequence = ClearTrashEnum.START;
				jogSequence = JogEnum.DETECT_TRASH;
			}
			
			if (jogSequence != JogEnum.DETECT_TRASH)
				break;
		case DETECT_TRASH:
			//if (performClearTrash() == 1) {
				jogSequence = JogEnum.DETECT_TREASURE;
			//}
			
			if (jogSequence != JogEnum.DETECT_TREASURE)
				break;
		case DETECT_TREASURE:
			if (m_treasureDetector.isPresent()) {
				log("Treasure Detected @ performJog", 1);
				pickSequence = PickEnum.START;
				mainSequence = RunEnum.PERFORM_PICK;
			}
			else {
				armUpSequence = ArmUpEnum.START;
				jogSequence = JogEnum.ENSURE_ARM_UP;
			}
			
			if (jogSequence != JogEnum.ENSURE_ARM_UP)
				break;
		case ENSURE_ARM_UP:
			if (ensureArmUp(true) == 1) {
				compassCompensationSequence = CompassCompensationEnum.START;
				jogSequence = JogEnum.COMPASS_COMPENSATION;
			}
			
			if (jogSequence != JogEnum.COMPASS_COMPENSATION)
				break;
		case COMPASS_COMPENSATION:
			if (useCompassCompensation) {
				if (performCompassCompensation(RobotAction.Jog) == 1)
					jogSequence = JogEnum.UPDATE_NEXT_ACTION;
			}
			else {
				jogSequence = JogEnum.UPDATE_NEXT_ACTION;
			}
			
			if (jogSequence != JogEnum.UPDATE_NEXT_ACTION)
				break;
		case UPDATE_NEXT_ACTION:
			m_robotPosition = m_positionDetector.getPosition();
			determineNextAction(true);
			
			break;
		case ADJUST:
			if (performAdjust() != 1)
				break;
			
			if (m_amountToAdjust == 0)
				jogSequence = JogEnum.WAIT;
			else
				jogSequence = JogEnum.START;

			break;
		case WAIT:
			log("Wait @ performJog", 4);
			//sleep(INTERVAL);
			jogSequence = JogEnum.DETECT_TREASURE;
			break;
		case DETECT_FRONT_WALL:
			performDetectFrontWall();
			break;
		case ERROR:
			break;
		default:
			jogSequence = JogEnum.START;
			break;
		}
	}
	
	private int performTurnRight() {
		return performTurn(RobotAction.TurnRight, IRobotCommander.RIGHT_90_DEGREE);
	}
	
	private int performTurnLeft() {
		return performTurn(RobotAction.TurnLeft, IRobotCommander.LEFT_90_DEGREE);
	}
	
	private int performTurnBack() {
		return performTurn(RobotAction.TurnBack, IRobotCommander._180_DEGREE);
	}
	
	/*
	private int performTurnBack() {
		switch (turnBackSequence)
		{
		case START:
			log("Start Turn Back @ performTurnBack", 2);
			turnBackSequence = TurnBackEnum.STOP_JOG;
		case STOP_JOG:
			if (performSendCommand(RobotAction.Stop) != 1)
				break;
			
			log("Stop Done @ performTurnBack", 2);
			
			armUpSequence = ArmUpEnum.START;
			turnBackSequence = TurnBackEnum.ENSURE_ARM_UP;
			break;
		case ENSURE_ARM_UP:
			if (ensureArmUp(false) == 1) {
				log("Ensure Arm Up Done @ performTurnBack", 2);
				turnBackSequence = TurnBackEnum.CHECK_IF_JUCTION_REACHED;
			}
			break;
		case CHECK_IF_JUCTION_REACHED:
			m_robotPosition = m_positionDetector.getPosition();
			
			if (checkForJunction(false)) {
				log("Junction Reached During Reverse @ performTurnBack", 2);
				turnBackSequence = TurnBackEnum.REVERSE_INTO_JUNCTION;
			}
			else {
				turnBackSequence = TurnBackEnum.REVERSE_UNTIL_JUNCTION_REACHED;
			}
			
			break;
		case REVERSE_UNTIL_JUNCTION_REACHED:
			if (performSendCommand(RobotAction.MoveBackward, UTURN_MOVE_INDEX) != 1)
				break;

			turnBackSequence = TurnBackEnum.CHECK_IF_JUCTION_REACHED;
			break;
		case REVERSE_INTO_JUNCTION:
			int distance = 0;
			
			if (m_sensorDetectedOpening == SensorID.Left)
				distance = LEFT_REVERSE_INTO_JUNCTION;
			else if (m_sensorDetectedOpening == SensorID.Right)
				distance = RIGHT_REVERSE_INTO_JUNCTION;
			
			if (performSendCommand(RobotAction.MoveBackward, distance) != 1)
				break;
			
			log("Reverse Into Junction Done @ performTurnBack", 2);
			
			sleep(MOTION_SETTLING_TIME);
			turnBackSequence = TurnBackEnum.CAPTURE_ORIENTATION_BEFORE_TURN;
			break;
		case CAPTURE_ORIENTATION_BEFORE_TURN:
		{
			m_curOrientation = (int)m_sensors.getOrientation();
			
			String msg = String.format("Orientation[%d] before U Turn @ performTurnBack", m_curOrientation);
			log(msg, 2);
			
			turnBackSequence = TurnBackEnum.TURN;
		}
		case TURN:
			if (performSendCommand(RobotAction.TurnBack) != 1)
				break;
			
			log("U Turn Done @ performTurnBack", 2);
			
			sleep(MOTION_SETTLING_TIME);
			turnBackSequence = TurnBackEnum.CHECK_ORIENTATION;
		case CHECK_ORIENTATION:
		{
			determineCompensationAngleAfterTurn(IRobotCommander._180_DEGREE);

			String msg = String.format("Compensation Angle[%d] after U Turn @ performTurnBack", m_compensationAngle);
			log(msg, 2);
			
			if (m_compensationAngle != 0)
				turnBackSequence = TurnBackEnum.COMPENSATE_TURN;
			else
				turnBackSequence = TurnBackEnum.CHECK_IF_OUT_OF_JUCNTION;
		}
			break;
		case COMPENSATE_TURN:
			if (performSendCommand(RobotAction.TurnDegree, m_compensationAngle) != 1)
				break;
			
			sleep(MOTION_SETTLING_TIME);
			turnBackSequence = TurnBackEnum.CHECK_ORIENTATION;
			break;
		case CHECK_IF_OUT_OF_JUCNTION:
			if (anyJunction()) {
				turnBackSequence = TurnBackEnum.MOVE_OUT_OF_JUNCTION;
			}
			else {
				log("Move Out Of Junction Done @ performTurnBack", 2);
				turnBackSequence = TurnBackEnum.POST_MOVE_OUT_OF_JUNCTION;
			}
			
			break;
		case MOVE_OUT_OF_JUNCTION:
			if (performSendCommand(RobotAction.MoveBackward, SMALL_MOVE_INDEX) != 1)
				break;

			turnBackSequence = TurnBackEnum.CHECK_IF_OUT_OF_JUCNTION;
			break;
		case POST_MOVE_OUT_OF_JUNCTION:
			if (performSendCommand(RobotAction.MoveBackward, SMALL_MOVE_INDEX) != 1)
				break;

			m_idealOrientation -= IRobotCommander._180_DEGREE;
			m_idealOrientation %= 360;
			
			m_solver.onJunctionPassed(RobotAction.TurnBack);
			
			turnBackSequence = TurnBackEnum.START;
			
			log("Post Move Out Of Junction Done @ performTurnBack", 2);
			return 1;
		case ERROR:
			break;
		default:
			turnBackSequence = TurnBackEnum.START;
			break;
		}
		
		return 0;
	}
	*/
	
	private int performTurn(RobotAction turnDirection, int degree) {
		switch (turnSequence)
		{
		case START:
		{
			String msg = String.format("Start Turn[%d] @ performTurn", degree);
			log(msg, 2);
			
			turnSequence = TurnEnum.STOP_JOG;
		}
		case STOP_JOG:
			if (performSendCommand(RobotAction.Stop) != 1)
				break;
			
			log("Stop Done @ performTurn", 2);
			
			armUpSequence = ArmUpEnum.START;
			turnSequence = TurnEnum.ENSURE_ARM_UP;
			
			break;
		case ENSURE_ARM_UP:
			if (ensureArmUp(false) == 1) {
				log("Ensure Arm Up Done @ performTurn", 2);
				
				m_robotPosition.frontwalldetected = m_sensors.detectFrontWall();
				
				if (m_robotPosition.frontwalldetected)
					turnSequence = TurnEnum.CHECK_TO_ENSURE_FRONT_SENSOR_OFF;
				else
					turnSequence = TurnEnum.DETERMINE_AMOUNT_TO_MOVE_WITHOUT_FRONT_WALL;
			}
			break;
		case CHECK_TO_ENSURE_FRONT_SENSOR_OFF:
			m_robotPosition.frontwalldetected = m_sensors.detectFrontWall();
			
			if (m_robotPosition.frontwalldetected) {
				turnSequence = TurnEnum.MOVE_UNTIL_FRONT_SENSOR_OFF;
			}
			else {
				log("Ensure Front Sensor OFF Done @ performTurn", 2);
				turnSequence = TurnEnum.MOVE_AFTER_FRONT_SENSOR_OFF;
			}
			
			break;
		case MOVE_UNTIL_FRONT_SENSOR_OFF:
			if (performSendCommand(RobotAction.MoveBackward, DETECT_FRONT_WALL_INDEX) != 1)
				break;

			turnSequence = TurnEnum.CHECK_TO_ENSURE_FRONT_SENSOR_OFF;
			break;
		case MOVE_AFTER_FRONT_SENSOR_OFF:
			if (performSendCommand(RobotAction.MoveBackward, FINAL_DETECT_FRONT_WALL_INDEX) != 1)
				break;

			sleep(MOTION_SETTLING_TIME);
			turnSequence = TurnEnum.CAPTURE_ORIENTATION_BEFORE_TURN;
			break;
		case DETERMINE_AMOUNT_TO_MOVE_WITHOUT_FRONT_WALL:
			checkForJunction(true);
			
			if (m_sensorDetectedOpening == SensorID.Left)
				m_amountToIndex = LEFT_MOVEBACKWARD_WITHOUT_FRONT_WALL;
			else if (m_sensorDetectedOpening == SensorID.Right)
				m_amountToIndex = RIGHT_MOVEBACKWARD_WITHOUT_FRONT_WALL;
			
			turnSequence = TurnEnum.MOVE_WITHOUT_FRONT_WALL;
			break;
		case MOVE_WITHOUT_FRONT_WALL:

			if (performSendCommand(RobotAction.MoveBackward, m_amountToIndex) != 1)
				break;

			log("Move Without Front Wall Done @ performTurn", 2);
			
			sleep(MOTION_SETTLING_TIME);
			turnSequence = TurnEnum.CAPTURE_ORIENTATION_BEFORE_TURN;
			break;
		case CAPTURE_ORIENTATION_BEFORE_TURN:
		{
			m_curOrientation = (int)m_sensors.getOrientation();
			
			String msg = String.format("Orientation[%d] before Turn @ performTurn", m_curOrientation);
			log(msg, 2);
			
			m_turn_direction = 1;
			
			if (degree > 0) {
				m_turn_angle = 90;
			}
			else {
				m_turn_angle = -90;
			}

			m_turn_count = degree / m_turn_angle;
			if (m_turn_count < 0)
				m_turn_count = -m_turn_count;
			
			if (turnDirection == RobotAction.TurnBack) {
				turnSequence = TurnEnum.DETERMINE_U_TURN_DIRECTION;
			}
			else {
				turnSequence = TurnEnum.TURN;
			}
		}
			break;
		case DETERMINE_U_TURN_DIRECTION:
			determineUTurnDirection();
			turnSequence = TurnEnum.TURN;
			break;
		case TURN:
		{
			if (performSendCommand(RobotAction.TurnDegree, m_turn_angle * m_turn_direction) != 1)
				break;

			String msg = String.format("Turn Done[%d,%d,%d] @ performTurn", m_turn_angle, m_turn_count, m_turn_direction);
			log(msg, 2);
			
			if (--m_turn_count > 0) {
				turnSequence = TurnEnum.TURN;
			}
			else {
				sleep(MOTION_SETTLING_TIME);
				turnSequence = TurnEnum.CHECK_ORIENTATION;
			}
		}
			break;
		case CHECK_ORIENTATION:
		{
			determineCompensationAngleAfterTurn(degree);

			if (m_compensationAngle != 0) {
				turnSequence = TurnEnum.COMPENSATE_TURN;
			}
			else {
				if (turnDirection == RobotAction.TurnBack) {
					log("Post Move Out Of Junction Done @ performTurn", 2);
					
					m_idealOrientation -= degree;
					m_idealOrientation %= 360;
					
					turnSequence = TurnEnum.START;
					return 1;
				}
				else {
					sendJog2Sequence = SendJog2Enum.SEND;
					turnSequence = TurnEnum.MOVE_OUT_OF_JUNCTION;
				}
			}
		}
			break;
		case COMPENSATE_TURN:
			if (performSendCommand(RobotAction.TurnDegree, m_compensationAngle) != 1)
				break;
			
			sleep(MOTION_SETTLING_TIME);
			turnSequence = TurnEnum.CHECK_ORIENTATION;
			break;
		case MOVE_OUT_OF_JUNCTION:
			if (performSendJog2Command() != 1)
				break;

			log("Move Out Of Junction Done @ performTurn", 2);
			
			turnSequence = TurnEnum.POST_MOVE_OUT_OF_JUNCTION;
			break;
		case POST_MOVE_OUT_OF_JUNCTION:
			if (performSendCommand(RobotAction.MoveForward, SMALL_MOVE_INDEX) != 1)
				break;
			
			log("Post Move Out Of Junction Done @ performTurn", 2);
			
			m_idealOrientation -= degree;
			m_idealOrientation %= 360;
			
			turnSequence = TurnEnum.START;
			return 1;
		case ERROR:
			break;
		default:
			turnSequence = TurnEnum.START;
			break;
		}
		
		return 0;
	}
	
	private int performPick() {
		switch (pickSequence)
		{
		case START:
			/*
			 * Initialize variables here.
			 */
			log("Start Pick @ performPick", 2);
			m_pickTreasureRetryCount = 0;
			pickSequence = PickEnum.STOP_ROBOT;
		case STOP_ROBOT:
			if (performSendCommand(RobotAction.Stop) != 1)
				break;
			
			log("Stop Done @ performPick", 2);
			
			sleep(MOTION_SETTLING_TIME);
			
			if (m_treasureDetector.isPresent()) {
				m_curOrientation = (int)m_sensors.getOrientation();
				pickSequence = PickEnum.GET_TREASURE_POSITION;
			}
			else {//False alarm or over travel?
				log("Treasure Missing! @ performPick", 2);
				pickSequence = PickEnum.MOVE_BACK_FOR_MISSING_TREASURE;
			}
			break;
		case MOVE_BACK_FOR_MISSING_TREASURE:
		{
			if (performSendCommand(RobotAction.MoveBackward, SMALL_MOVE_INDEX) != 1)
				break;

			sleep(MOTION_SETTLING_TIME2);
			
			if (m_treasureDetector.isPresent()) {
				log("Treasure Foud :) @ performPick", 2);
				pickSequence = PickEnum.GET_TREASURE_POSITION;
			}
			else {
				log("Treasure Missing! Continue Index back. @ performPick", 2);
			}
		}
			break;
		case GET_TREASURE_POSITION:
			m_treasureDrift = m_treasureDetector.getDrift();

			if (m_treasureDrift.y != 0) {
				pickSequence = PickEnum.INDEX_TO_HORIZONTAL_PICK_POSITION;
			}
			else {
				log("Locate Treasure at Horizontal Pick Center Done @ performPick", 2);
				pickSequence = PickEnum.MOVE_BACK_BEFORE_ARM_DOWN;
			}
			break;
		case INDEX_TO_HORIZONTAL_PICK_POSITION:
		{
			if (performSendCommand(RobotAction.TurnDegree, m_treasureDrift.y > 0? SMALL_TURN_INDEX : -SMALL_TURN_INDEX) != 1)
				break;
			
			String msg = String.format("Turn[%d] to Horizontal Pick Center Done @ performPick", m_treasureDrift.y > 0? SMALL_TURN_INDEX : -SMALL_TURN_INDEX);
			log(msg, 2);

			pickSequence = PickEnum.GET_TREASURE_POSITION;
		}
			break;
		case MOVE_BACK_BEFORE_ARM_DOWN:
		{
			if (performSendCommand(RobotAction.MoveBackward, MOVE_BACK_BEFORE_ARM_DOWN_DIST) != 1)
				break;
			
			String msg = String.format("Move Back before arm down Done[%d] @ performPick", MOVE_BACK_BEFORE_ARM_DOWN_DIST);
			log(msg, 2);

			pickSequence = PickEnum.ARM_DOWN;
		}
			break;
		case ARM_DOWN:
		{
			if (performSendCommand(RobotAction.ArmDown) != 1)
				break;
			
			sleep(MOTION_SETTLING_TIME2);
			
			log("Arm Down Done @ performPick", 2);
			
			m_treasureDrift = m_treasureDetector.getDrift();
			
			String msg = String.format("Before Jog Driftx[%d] @ performPick", (int)m_treasureDrift.x);
			log(msg, 2);
			
			if (m_treasureDetector.isPresent())
				pickSequence = PickEnum.CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK;
			else
				pickSequence = PickEnum.MOVE_FORWARD_TO_DETECT_TREASURE;
		}
			break;
		case MOVE_FORWARD_TO_DETECT_TREASURE:
		{
			if (performSendCommand(RobotAction.MoveForward, SMALL_MOVE_INDEX) != 1)
				break;
			
			if (m_treasureDetector.isPresent())
				pickSequence = PickEnum.CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK;
		}
			break;
		case JOG_TO_VERTICAL_PICK_POSITION:
			if (performSendCommand(RobotAction.Jog) != 1)
				break;
			
			log("Start Jog @ performPick", 2);
			
			pickSequence = PickEnum.CHECK_FOR_TREASURE_BOX;
			break;
		case CHECK_FOR_TREASURE_BOX:
		{
			m_treasureDrift = m_treasureDetector.getDrift();

			if (m_treasureDrift.x >= -0.2f) {
				
				String msg = String.format("Drift x[%d]", (int)m_treasureDrift.x);
				log(msg, 2);
				
				pickSequence = PickEnum.STOP_JOG_TO_VERTICAL_PICK_POSITION;
			}
			else {
				sleep(INTERVAL);
			}
		}
			break;
		case STOP_JOG_TO_VERTICAL_PICK_POSITION:
			if (performSendCommand(RobotAction.Stop) != 1)
				break;
			
			log("Stop Jog @ performPick", 2);
			
			pickSequence = PickEnum.CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK;
			break;
		case CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK:
			if (m_treasureDetector.isPresent()) {
				pickSequence = PickEnum.INDEX_TO_VERTICAL_PICK_POSITION;
			}
			else {
				log("Treasure Absent @ performPick", 2);
				pickSequence = PickEnum.FINAL_INDEX_TO_VERTICAL_PICK_POSITION;
			}

			break;
		case INDEX_TO_VERTICAL_PICK_POSITION:
			if (performSendCommand(RobotAction.Move, DETECT_TREASURE_ABSCENT_MOVE_INDEX) != 1)
				break;

			pickSequence = PickEnum.CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK;
			break;
		case FINAL_INDEX_TO_VERTICAL_PICK_POSITION:
			if (performSendCommand(RobotAction.Move, PICK_TREASURE_FINAL_MOVE) != 1)
				break;
			
			log("Final Index Done @ performPick", 2);
			
			pickSequence = PickEnum.PICK;
			break;
		case PICK:
			if (performSendPickCommand(RobotAction.PickTreasure) != 1)
				break;
			
			sleep(MOTION_SETTLING_TIME2);
			
			log("Pick Done @ performPick", 2);

			pickSequence = PickEnum.MOVE_BACK_TO_ENSURE_PICK_AREA_WITHIN_FOV;
		case MOVE_BACK_TO_ENSURE_PICK_AREA_WITHIN_FOV:
			if (performSendCommand(RobotAction.MoveBackward, MOVE_BACK_TO_ENSURE_PICK_AREA_WITHIN_FOV_DIST) != 1)
				break;
			
			log("Move Backward Done @ performPick", 2);

			armUpSequence = ArmUpEnum.START;
			pickSequence = PickEnum.ENSURE_ARM_UP_AFTER_PICK;
		case ENSURE_ARM_UP_AFTER_PICK:
			if (ensureArmUp(false) == 1) {
				log("Ensure Arm Up Done @ performPick", 2);
				sleep(MOTION_SETTLING_TIME);
				pickSequence = PickEnum.VERIFY_TREASURE_PICKED;
			}
			break;
		case VERIFY_TREASURE_PICKED:
			if (m_treasureDetector.isPresent() && (++m_pickTreasureRetryCount <= MAX_PICK_TREASURE_RETRY)) {
				log("Pick Treasure Failed! Retry. @ performPick", 2);
				pickSequence = PickEnum.GET_TREASURE_POSITION;
				break;
			}
			else {
				pickSequence = PickEnum.CHECK_DRIFT_FROM_INITIAL_ORIENTATION;
			}
		case CHECK_DRIFT_FROM_INITIAL_ORIENTATION:
			determineCompensationAngleAfterTurn(0);
			
			String msg = String.format("Compensation Angle[%d] after Turn @ performPick", m_compensationAngle);
			log(msg, 2);
			
			if (m_compensationAngle != 0) {
				pickSequence = PickEnum.TURN_TO_INITIAL_ORIENTATION_BEFORE_U_TURN;
			}
			else {
				pickSequence = PickEnum.JOG_TO_DEAD_END;
			}
			
			break;
		case TURN_TO_INITIAL_ORIENTATION_BEFORE_U_TURN:
			if (performSendCommand(RobotAction.TurnDegree, m_compensationAngle) != 1)
				break;
			
			sleep(MOTION_SETTLING_TIME);
			pickSequence = PickEnum.CHECK_DRIFT_FROM_INITIAL_ORIENTATION;
			break;
		case JOG_TO_DEAD_END:
			if (performSendCommand(RobotAction.Jog, m_idealOrientation) != 1)
				break;
			
			pickSequence = PickEnum.WAIT_JOG_TO_DEAD_END_DONE;
		case WAIT_JOG_TO_DEAD_END_DONE:
			if (!m_sensors.detectFrontWall())
				break;
			
			if (peekCommandDone(CMD_TIME_OUT) != 1)
				break;
			
			m_robotPosition.frontwalldetected = true;
			
			log("Move To Dead End Done. @ performPick", 2);
			
			//turnBackSequence = TurnBackEnum.START;
			turnBackSequence = TurnEnum.START;
			pickSequence = PickEnum.U_TURN_AFTER_PICK;
			break;
		case U_TURN_AFTER_PICK:
			if (performTurnBack() != 1)
				break;

			log("U Turn Done @ performPick", 2);
			pickSequence = PickEnum.START;
			return 1;
		case ERROR:
			break;
		default:
			pickSequence = PickEnum.START;
			break;	
		}
		
		return 0;
	}
	
	private int performCompassCompensation(RobotAction robotAction) {
		switch (compassCompensationSequence)
		{
		case START:
			compassCompensationSequence = CompassCompensationEnum.DETERMINE_ADJUST_TURN_ANGLE;
		case DETERMINE_ADJUST_TURN_ANGLE:
			m_curOrientation = m_idealOrientation;
			determineCompensationAngleAfterTurn(0);
			
			if (m_compensationAngle != 0) {
				// try to over compensate
				if (m_compensationAngle < 0)
					m_compensationAngle -= COMPENSATION_ANGLE_TOLERANCE * 2;
				else
					m_compensationAngle += COMPENSATION_ANGLE_TOLERANCE * 2;
				
				String msg = String.format("CompassCompensation[%d,%d,%d] @ performCompassCompensation", m_curOrientation, m_compensationAngle, (int)m_sensors.getOrientation());
				log(msg, 2);
			}
			
			if (m_compensationAngle == 0) {
				compassCompensationSequence = CompassCompensationEnum.START;
				return 1;
			}
			else {
				compassCompensationSequence = CompassCompensationEnum.STOP_ROBOT;
			}
			break;
		case STOP_ROBOT:
			if (performSendCommand(RobotAction.Stop) != 1)				
				break;
			
			log("Stop Done @ performCompassCompensation", 2);
			compassCompensationSequence = CompassCompensationEnum.ADJUST;
			break;
		case ADJUST:
			if (performSendCommand(RobotAction.TurnDegree, m_compensationAngle) != 1)			
				break;
			
			sleep(MOTION_SETTLING_TIME);
			
			log("Adjust Done @ performCompassCompensation", 2);
			compassCompensationSequence = CompassCompensationEnum.RESUME_CMD;
			break;
		case RESUME_CMD:
			if (robotAction == RobotAction.Jog2) {
				if (!sendCommand(RobotAction.Jog2, m_idealOrientation))
					return -1;
			}
			else {
				if (performSendCommand(robotAction) != 1)				
					break;
			}
			
			log("RESUME CMD Done @ performCompassCompensation", 2);
			compassCompensationSequence = CompassCompensationEnum.START;
			return 1;
		case ERROR:
			break;
		default:
			compassCompensationSequence = CompassCompensationEnum.START;
			break;
		}
		
		return 0;
	}
	
	private int performAdjust() {
		switch (adjustSequence)
		{
		case START:
			adjustSequence = AdjustEnum.DETERMINE_ADJUST_TURN_ANGLE;
		case DETERMINE_ADJUST_TURN_ANGLE:
			int angle = determineAdjustAngle();
			if (m_adjustAngle != angle) {
				m_amountToAdjust = angle - m_adjustAngle;
				m_adjustAngle = angle;
				
				String msg = String.format("AmountToTurn[%d], Adjust[%d] @ performAdjust", m_amountToAdjust, m_adjustAngle);
				log(msg, 2);
			}
			else {
				m_amountToAdjust = 0;
			}
			
			if (m_amountToAdjust == 0) {
				adjustSequence = AdjustEnum.START;
				return 1;
			}
			else {
				adjustSequence = AdjustEnum.STOP_JOG;
			}
			break;
		case STOP_JOG:
			if (performSendCommand(RobotAction.Stop) != 1)				
				break;
			
			log("Stop Done @ performAdjust", 2);
			adjustSequence = AdjustEnum.ADJUST;
			break;
		case ADJUST:
			if (performSendCommand(RobotAction.TurnDegree, m_amountToAdjust) != 1)			
				break;
			
			log("Adjust Done @ performAdjust", 2);
			adjustSequence = AdjustEnum.START_JOG;
			break;
		case START_JOG:
			if (performSendCommand(RobotAction.Jog, m_idealOrientation) != 1)				
				break;
			
			log("Jog Done @ performAdjust", 2);
			adjustSequence = AdjustEnum.START;
			return 1;
		case ERROR:
			break;
		default:
			adjustSequence = AdjustEnum.START;
			break;
		}
		return 0;
	}
	
	private int performAdjustBack() {
		switch (adjustBackSequence)
		{
		case START:
			adjustBackSequence = AdjustEnum.DETERMINE_ADJUST_TURN_ANGLE;
		case DETERMINE_ADJUST_TURN_ANGLE:

			if (m_adjustAngle == 0) {
				adjustBackSequence = AdjustEnum.START;
				return 1;
			}
			
			adjustBackSequence = AdjustEnum.STOP_JOG;
		case STOP_JOG:
			if (performSendCommand(RobotAction.Stop) != 1)
				break;
			
			log("Stop Done @ performAdjustBack", 2);
			
			adjustBackSequence = AdjustEnum.ADJUST;
			break;	
		case ADJUST:
			if (performSendCommand(RobotAction.TurnDegree, -m_adjustAngle) != 1)
				break;
			
			String msg = String.format("Adjust Back[%d] @ performAdjustBack", -m_adjustAngle);
			log(msg, 2);
			
			m_adjustAngle = 0;
			adjustBackSequence = AdjustEnum.START;
			return 1;
		case ERROR:
			break;
		default:
			adjustBackSequence = AdjustEnum.START;
			break;
		}
		return 0;
	}
	
	private int ensureArmUp(boolean isJogging) {
		switch (armUpSequence)
		{
		case START:
			armUpSequence = ArmUpEnum.CHECK_UP;
		case CHECK_UP:
			if (m_sensors.isArmUp()) {
				armUpSequence = ArmUpEnum.START;
				return 1;
			}
			else {
				if (isJogging)
					armUpSequence = ArmUpEnum.STOP_JOG;
				else
					armUpSequence = ArmUpEnum.REVERSE;
			}
			break;
		case STOP_JOG:
			if (performSendCommand(RobotAction.Stop) != 1)		
				break;

			log("Stop Done @ ensureArmUp", 2);
			armUpSequence = ArmUpEnum.REVERSE;
			break;
		case REVERSE:
			if (performSendCommand(RobotAction.MoveBackward, MOVE_INDEX) != 1)				
				break;
				
			log("Reverse Done @ ensureArmUp", 2);
			armUpSequence = ArmUpEnum.MOVE_UP;
			break;
		case MOVE_UP:
			if (performSendPickCommand(RobotAction.PickTreasure) != 1)				
				break;
			
			log("Arm Up Done @ ensureArmUp", 2);
			armUpSequence = ArmUpEnum.FORWARD;
			break;
		case FORWARD:
			if (performSendCommand(RobotAction.MoveForward, MOVE_INDEX) != 1)				
				break;

			log("Forward Done @ ensureArmUp", 2);
			
			if (isJogging)
				armUpSequence = ArmUpEnum.START_JOG;
			else
				armUpSequence = ArmUpEnum.CHECK_UP;
			
			break;
		case START_JOG:
			if (performSendCommand(RobotAction.Jog, m_idealOrientation) != 1)
				break;
			
			log("Jog Done @ ensureArmUp", 2);
			armUpSequence = ArmUpEnum.START;
			return 1;
		case ERROR:
			break;
		default:
			armUpSequence = ArmUpEnum.START;
			break;
		}
		return 0;
	}
	
	private void performDetectFrontWall() {
		switch (detectFrontWallSequence)
		{
		case START:
			/*
			 * Initialize variables here.
			 */
			detectFrontWallSequence = DetectFrontWallEnum.STOP_JOG;
		case STOP_JOG:
			if (performSendCommand(RobotAction.Stop) != 1)
				break;
			
			log("Stop Done @ performDetectFrontWall", 2);

			adjustBackSequence = AdjustEnum.START;
			detectFrontWallSequence = DetectFrontWallEnum.ADJUST_BACK;
			break;
		case ADJUST_BACK:
			if (performAdjustBack() == 1) {
				log("Adjust Back Done @ performDetectFrontWall", 2);
				armUpSequence = ArmUpEnum.START;
				detectFrontWallSequence = DetectFrontWallEnum.ENSURE_ARM_UP;
			}
			break;
		case ENSURE_ARM_UP:
			if (ensureArmUp(false) == 1) {
				log("Ensure Arm Up Done @ performDetectFrontWall", 2);
				sendJog2Sequence = SendJog2Enum.SEND;
				detectFrontWallSequence = DetectFrontWallEnum.MOVE_FORWARD_SIDE_SENSOR_OFFSET_DIST;
			}
			break;
		case MOVE_FORWARD_SIDE_SENSOR_OFFSET_DIST:
			if (performSendCommand(RobotAction.MoveForward, SIDE_SENSOR_OFFSET) != 1)
				break;
			
			m_robotPosition = m_positionDetector.getPosition();
			detectFrontWallSequence = DetectFrontWallEnum.INDEX_TO_DETECT_FRONT_WALL;
			break;
		case INDEX_TO_DETECT_FRONT_WALL:
		{
			if (performSendJog2Command() != 1)
				break;
			
			m_robotPosition.frontwalldetected = m_sensors.detectFrontWall();

			String msg = String.format("Detect Wall Done. Front Wall[%s] Location[%s] @ performDetectFrontWall", 
									m_robotPosition.frontwalldetected? "YES":"NO",
									m_robotPosition.location.name());
			
			log(msg, 2);
			detectFrontWallSequence = DetectFrontWallEnum.UPDATE_NEXT_ACTION;
		}
			break;
		case UPDATE_NEXT_ACTION:
		{
			updateNextActionAfterDetectWall();
			String msg = String.format("Update Next Action Done. Location[%s] @ performDetectFrontWall", m_robotPosition.location.name());
			log(msg, 2);
		}
			break;
		case CONTINUE_JOGGING:
			if (performSendCommand(RobotAction.MoveForward, SMALL_MOVE_INDEX) != 1)
				break;

			if (!anyJunction()) {
				log("Continue Jogging Done @ performDetectFrontWall", 2);
				detectFrontWallSequence = DetectFrontWallEnum.POST_MOVE_FOR_CONTINUE_JOGGING;
			}

			break;
		case POST_MOVE_FOR_CONTINUE_JOGGING:
			if (performSendCommand(RobotAction.MoveForward, SMALL_MOVE_INDEX) != 1)
				break;
			
			m_solver.onJunctionPassed(RobotAction.Jog);
			
			log("Post Move for Continue Jogging Done @ performDetectFrontWall", 2);
			jogSequence = JogEnum.START;
			detectFrontWallSequence = DetectFrontWallEnum.START;
		case ERROR:
			break;
		default:
			detectFrontWallSequence = DetectFrontWallEnum.START;
			break;
		}
	}
	
	private int performClearTrash() {
		switch (clearTrashSequence)
		{
		case START:
		{
			if (!m_trashDetector.isPresent())
				return 1;

			String msg;
			Point center = m_trashDetector.getCenter();
			msg = String.format("Trash detected[%d,%d]! @ performClearTrash", (int)center.x, (int)center.y);
			log(msg, 1);
			
			clearTrashSequence = ClearTrashEnum.STOP_ROBOT;
		}
		case STOP_ROBOT:
			if (performSendCommand(RobotAction.Stop) != 1)
				break;
			
			log("Send Stop Done @ performClearTrash", 2);
			clearTrashSequence = ClearTrashEnum.JOG_TO_VERTICAL_RETURN_POSITION;
		case JOG_TO_VERTICAL_RETURN_POSITION:
			if (performSendCommand(RobotAction.Jog) != 1)
				break;
			
			log("Start Jog to return position @ performClearTrash", 2);
			
			clearTrashSequence = ClearTrashEnum.CHECK_FOR_TRASH_BOX;
			break;
		case CHECK_FOR_TRASH_BOX:
		{
			m_trashDrift = m_trashDetector.getDrift();

			if (m_trashDrift.x >= 0) {
				
				String msg = String.format("Drift x[%d]", (int)m_trashDrift.x);
				log(msg, 2);
				
				clearTrashSequence = ClearTrashEnum.U_TURN;
			}
			else {
				sleep(INTERVAL);
			}
		}
		case U_TURN:
			if (performSendCommand(RobotAction.TurnBack) != 1)
				break;

			clearTrashSequence = ClearTrashEnum.START_JOG;
			break;
		case START_JOG:
			if (performSendCommand(RobotAction.Jog) != 1)				
				break;

			log("RESUME CMD Done Done @ performClearTrash", 2);
			
			clearTrashSequence = ClearTrashEnum.START;
			return 1;
		case ERROR:
			break;
		default:
			clearTrashSequence = ClearTrashEnum.START;
			break;
		}
		
		return 0;
	}
	
	private int performClearBarrier(RobotAction robotAction) {
		switch (clearBarrierSequence)
		{
		case START:
		{
			if (!m_barrierDetector.isPresent())
				return 1;

			String msg;
			Point center = m_barrierDetector.getCenter();
			msg = String.format("Barrier detected[%d,%d]! @ performClearBarrier", (int)center.x, (int)center.y);
			log(msg, 1);
			
			clearBarrierSequence = ClearBarrierEnum.STOP_ROBOT;
		}
		case STOP_ROBOT:
			if (performSendCommand(RobotAction.Stop) != 1)
				break;
			
			log("Send Stop Done @ performClearBarrier", 2);
			clearBarrierSequence = ClearBarrierEnum.WAIT;
		case WAIT:
			sleep(INTERVAL);
			
			if (!m_barrierDetector.isPresent()) {
				sleep(MOTION_SETTLING_TIME2);
				clearBarrierSequence = ClearBarrierEnum.RESUME_CMD;
			}
			
			break;
		case RESUME_CMD:
			if (robotAction == RobotAction.Jog2) {
				if (!sendCommand(RobotAction.Jog2, m_idealOrientation))
					return -1;
			}
			else {
				if (performSendCommand(robotAction, m_idealOrientation) != 1)				
					break;
			}

			log("RESUME CMD Done Done @ performClearBarrier", 2);
			
			clearBarrierSequence = ClearBarrierEnum.START;
			return 1;
		case ERROR:
			break;
		default:
			clearBarrierSequence = ClearBarrierEnum.START;
			break;
		}
		
		return 0;
	}
	
	private void determineNextAction(boolean beforeDetectWall) {
		RobotCommand command = m_solver.nextAction(m_robotPosition, beforeDetectWall);
		
		switch (command.action) {
		case Jog:
			if (beforeDetectWall) {
				//logSensors();
				
				adjustSequence = AdjustEnum.START;
				jogSequence = JogEnum.ADJUST;
				mainSequence = RunEnum.PERFORM_JOG;
			}
			else {
				log("Continue Jogging @ determineNextAction", 2);
				detectFrontWallSequence = DetectFrontWallEnum.CONTINUE_JOGGING;
				jogSequence = JogEnum.DETECT_FRONT_WALL;
				mainSequence = RunEnum.PERFORM_JOG;
			}
			break;
		case TurnRight:
			log("Turn Right @ determineNextAction", 2);
			detectFrontWallSequence = DetectFrontWallEnum.START;
			jogSequence = JogEnum.START;
			turnSequence = TurnEnum.START;
			mainSequence = RunEnum.PERFORM_TURN_RIGHT;
			break;
		case TurnLeft:
			log("Turn Left @ determineNextAction", 2);
			detectFrontWallSequence = DetectFrontWallEnum.START;
			jogSequence = JogEnum.START;
			turnSequence = TurnEnum.START;
			mainSequence = RunEnum.PERFORM_TURN_LEFT;
			break;
		case TurnBack:
			log("Turn Back @ determineNextAction", 2);
			detectFrontWallSequence = DetectFrontWallEnum.START;
			jogSequence = JogEnum.START;
			turnSequence = TurnEnum.START;
			mainSequence = RunEnum.PERFORM_TURN_BACK;
			break;
		case Detect_Front_Wall:
			if (!beforeDetectWall)
				assert(false);
			
			String msg = String.format("Need Detect Front Wall. Location[%s] @ performDetectFrontWall", m_robotPosition.location.name());
			log(msg, 2);

			detectFrontWallSequence = DetectFrontWallEnum.START;
			jogSequence = JogEnum.DETECT_FRONT_WALL;
			mainSequence = RunEnum.PERFORM_JOG;
			break;
		default:
			assert(false);
			break;
		}
	}
	
	private void updateNextActionAfterDetectWall() {

		if (m_robotPosition.frontwalldetected) {
			switch (m_robotPosition.location) 
			{
			case TRightStraightButTooLeft2:
				m_robotPosition.location = RobotLocation.RightTurnOnlyButTooLeft2;
				break;
			case TRightStraightButTooLeft1:
				m_robotPosition.location = RobotLocation.RightTurnOnlyButTooLeft1;
				break;
			case TRightStraight:
				m_robotPosition.location = RobotLocation.RightTurnOnly;
				break;
			case TLeftStraightButTooRight2:
				m_robotPosition.location = RobotLocation.LeftTurnOnlyButTooRight2;
				break;
			case TLeftRStraightButTooRight1:
				m_robotPosition.location = RobotLocation.LeftTurnOnlyButTooRight1;
				break;
			case TLeftStraight:
				m_robotPosition.location = RobotLocation.LeftTurnOnly;
				break;
			case FourWay:
				m_robotPosition.location = RobotLocation.TLeftRight;
				break;
				//hack
			case LaneLeft1:
				m_robotPosition.location = RobotLocation.RightTurnOnlyButTooLeft1;
				break;
			case LaneLeft2:
				m_robotPosition.location = RobotLocation.RightTurnOnlyButTooLeft2;
				break;
			case LaneRight1:
				m_robotPosition.location = RobotLocation.LeftTurnOnlyButTooRight1;
				break;
			case LaneRight2:
				m_robotPosition.location = RobotLocation.LeftTurnOnlyButTooRight2;
				break;
			default:
				break;
			}
		}

		determineNextAction(false);
	}
	
	private boolean anyJunction() {
		RobotPosition robotPosition = m_positionDetector.getPosition();
		
		switch (robotPosition.location) 
		{
		case RightTurnOnlyButTooLeft2:
		case RightTurnOnlyButTooLeft1:
		case RightTurnOnly:
		case TRightStraightButTooLeft2:
		case TRightStraightButTooLeft1:
		case TRightStraight:
		case LeftTurnOnlyButTooRight2:
		case LeftTurnOnlyButTooRight1:
		case LeftTurnOnly:
		case TLeftStraightButTooRight2:
		case TLeftRStraightButTooRight1:
		case TLeftStraight:
		case TLeftRight:
		case FourWay:
			return true;
		default:
			break;
		}

		return false;
	}
	
	private boolean checkForJunction(boolean forward) {
		m_sensorDetectedOpening = SensorID.Max;
		
		switch (m_robotPosition.location) 
		{
		case RightTurnOnlyButTooLeft2:
		case RightTurnOnlyButTooLeft1:
		case RightTurnOnly:
		case TRightStraightButTooLeft2:
		case TRightStraightButTooLeft1:
		case TRightStraight:
			m_sensorDetectedOpening = SensorID.Right;
			return true;
		case LeftTurnOnlyButTooRight2:
		case LeftTurnOnlyButTooRight1:
		case LeftTurnOnly:
		case TLeftStraightButTooRight2:
		case TLeftRStraightButTooRight1:
		case TLeftStraight:
			m_sensorDetectedOpening = SensorID.Left;
			return true;
		case TLeftRight:
		case FourWay:
			if (forward)
				m_sensorDetectedOpening = SensorID.Left;
			else
				m_sensorDetectedOpening = SensorID.Right;
			return true;
		default:
			break;
		}

		return false;
	}
	
	private void determineUTurnDirection() {
		
		m_turn_direction = 1;
		
		RobotPosition robotPosition = m_positionDetector.getPosition();
		if (RobotPositionDetector.isTooLeft(robotPosition)) {
			m_turn_direction = -1;
		}
	}
	
	private void determineCompensationAngleAfterTurn(int angleTurned) {
		int expectedOrientation = m_curOrientation - angleTurned;
		int curOrientation = (int)m_sensors.getOrientation();

		String msg = String.format("Old orientation[%d], new orientation[%d], angle to turn[%d] @ performTurn", 
				m_curOrientation, curOrientation, angleTurned);
		log(msg, 2);
		
		m_compensationAngle = curOrientation - expectedOrientation;
		
		m_compensationAngle %= 360;
		
		if (m_compensationAngle > 180)
			m_compensationAngle -= 360;
		else if (m_compensationAngle < -180)
			m_compensationAngle += 360;
		
		if (Math.abs(m_compensationAngle) < COMPENSATION_ANGLE_TOLERANCE)
			m_compensationAngle = 0;
		
		if (!useCompassCompensationDuringTurn)
			m_compensationAngle = 0;

		msg = String.format("Compensation Angle[%d] after Turn @ performTurn", m_compensationAngle);
		log(msg, 2);
	}
	
	private int determineAdjustAngle() {
		int angle = 0;
		
		switch(m_robotPosition.location)
		{
		case LaneLeft2:
			angle = -ADJUST_ANGLE;
			break;
		case LaneLeft1:
			if (m_adjustAngle != 0)
				angle = -ADJUST_ANGLE;
			break;
		case LaneRight2:
			angle = ADJUST_ANGLE;
			break;
		case LaneRight1:
			if (m_adjustAngle != 0)
				angle = ADJUST_ANGLE;
			break;
		default:
			break;
		}

		return angle;
	}
	
	private int performSendCommand(RobotAction robotAction)  {
		return performSendCommand(robotAction, 0, CMD_TIME_OUT);
	}
	
	private int performSendCommand(RobotAction robotAction, int param)  {
		return performSendCommand(robotAction, param, CMD_TIME_OUT);
	}
	
	private int performSendPickCommand(RobotAction robotAction)  {
		return performSendCommand(robotAction, 0, PICK_TIME_OUT);
	}
	
	private int performSendJog2Command() {
		switch(sendJog2Sequence)
		{
		case SEND:
		{
			if (m_barrierDetector.isPresent()) {
				clearBarrierSequence = ClearBarrierEnum.START;
				sendJog2Sequence = SendJog2Enum.DETECT_BARRIER;
			}
			else {
				if (!sendCommand(RobotAction.Jog2, m_idealOrientation)) {
					sendJog2RecoverSequence = SendJog2Enum.SEND;
					sendJog2Sequence = SendJog2Enum.ERROR;
					break;
				}
	
				String msg = String.format("Send Jog2 done.");
				log(msg, 2);
				
				clearBarrierSequence = ClearBarrierEnum.START;
				sendJog2Sequence = SendJog2Enum.DETECT_BARRIER;
			}
		}
		case DETECT_BARRIER:
			if (performClearBarrier(RobotAction.Jog2) == 1) {
				compassCompensationSequence = CompassCompensationEnum.START;
				sendJog2Sequence = SendJog2Enum.COMPASS_COMPENSATION;
			}
			
			if (sendJog2Sequence != SendJog2Enum.COMPASS_COMPENSATION)
				break;
		case COMPASS_COMPENSATION:
			if (useCompassCompensation) {
				int status = performCompassCompensation(RobotAction.Jog2);
				
				if (status == 1) {
					sendJog2Sequence = SendJog2Enum.WAIT;
				}
				else if (status == -1) {
					sendJog2RecoverSequence = SendJog2Enum.SEND;
					sendJog2Sequence = SendJog2Enum.ERROR;
				}
			}
			else {
				sendJog2Sequence = SendJog2Enum.WAIT;
			}

			if (sendJog2Sequence != SendJog2Enum.WAIT)
				break;
		case WAIT:
		{
			int status = peekCommandDone(CMD_TIME_OUT);
			
			if (status == 1) {
				String msg = String.format("Wait Jog2 done.");
				log(msg, 2);
				
				sendJog2Sequence = SendJog2Enum.SEND;
				return 1;
			}
			else if (status == -1) {
				sendJog2RecoverSequence = SendJog2Enum.SEND;
				sendJog2Sequence = SendJog2Enum.ERROR;
			}
			else {
				clearBarrierSequence = ClearBarrierEnum.START;
				sendJog2Sequence = SendJog2Enum.DETECT_BARRIER;
				break;
			}
		}
			break;
		case ERROR:
		{
			reconnectModbus();
			
			sleep(INTERVAL);
			
			String msg = String.format("Send Jog2 Failed @ performSendJog2Command");
			log(msg, 0);
			
			sendJog2Sequence = sendJog2RecoverSequence;
		}
			break;
		default:
			sendJog2Sequence = SendJog2Enum.SEND;
			break;
		}
		
		return 0;
	}
	
	private int performSendCommand(RobotAction robotAction, int param, int timeout) {
		switch(sendCommandSequence)
		{
		case SEND:
		{
			if (!sendCommand(robotAction, param)) {
				sendCommandRecoverSequence = SendCommandEnum.SEND;
				sendCommandSequence = SendCommandEnum.ERROR;
				break;
			}

			String msg = String.format("Send command %s[%d] done.", robotAction.name(), param);
			log(msg, 4);
			
			if (robotAction == RobotAction.Jog) {
				sendCommandSequence = SendCommandEnum.SEND;	
				return 1;
			}
			
			sendCommandSequence = SendCommandEnum.WAIT;
		}
		case WAIT:
		{
			int status;
			
			//if (robotAction == RobotAction.Jog)
			//	status = peekJogDone(timeout);
			//else
				status = peekCommandDone(timeout);
			
			if (status == 1) {
				String msg = String.format("Wait command %s done.", robotAction.name());
				log(msg, 4);
				
				sendCommandSequence = SendCommandEnum.SEND;
				return 1;
			}
			else if (status == -1) {
				sendCommandRecoverSequence = SendCommandEnum.SEND;
				sendCommandSequence = SendCommandEnum.ERROR;
			}
		}
			break;
		case ERROR:
		{
			reconnectModbus();
			
			sleep(INTERVAL);
			
			String msg = String.format("Send Command[%s] Failed @ performSendCommand", robotAction.name());
			log(msg, 0);
			
			sendCommandSequence = sendCommandRecoverSequence;
		}
			break;
		default:
			sendCommandSequence = SendCommandEnum.SEND;
			break;
		}
		
		return 0;
	}
	
	private boolean sendCommand(RobotAction robotAction, int param) {
		try {
			m_cmdTimeOut = 0;
			m_command.param1 = param;
			m_command.action = robotAction;
	
			m_commander.sendCommand(m_command);
			return true;
		}
		catch (Exception ex) {
			//m_gameOver = true;
			log(ex, 1);
			return false;
		}
	}
	
	private void reconnectModbus() {
		try {
			m_commander.disconnect();
			m_commander.connect();
		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	
	private int peekCommandDone(int timout) {
		try {
			if (m_commander.isCommandDone()) {
				return 1;
			}
			else if (m_cmdTimeOut < timout){
				m_cmdTimeOut += INTERVAL;
				sleep(INTERVAL);
				return 0;
			}
			else {
				return -1;
			}
		}
		catch (Exception ex) {
			//m_gameOver = true;
			log(ex, 1);
			
			return -1;
		}
	}
	
	private int peekJogDone(int timout) {
		try {
			if (m_commander.isJogDone()) {
				return 1;
			}
			else if (m_cmdTimeOut < timout){
				m_cmdTimeOut += INTERVAL;
				sleep(INTERVAL);
				return 0;
			}
			else {
				return -1;
			}
		}
		catch (Exception ex) {
			//m_gameOver = true;
			log(ex, 1);
			
			return -1;
		}
	}
	
	private void sleep(int duration) {
		try {
			Thread.sleep(duration);
		} catch (InterruptedException ex) {
			log(ex, 1);
		}
	}
	
	public void testContinueMission(Deque<RunEnum> actions) {
		jogSequence = JogEnum.START;
		mainSequence = RunEnum.PERFORM_JOG;
		RunEnum mainPreSequence = RunEnum.PERFORM_JOG;
		while(true) {
			continueMission();
			
			if (mainPreSequence != mainSequence)
				mainPreSequence = mainSequence;
			
			if (actions.getFirst() == mainSequence)
				actions.pop();
			
			if (actions.isEmpty())
				break;
		}
	}
	
	public void testJog() {
		sleep(MOTION_SETTLING_TIME);
		m_idealOrientation = (int)m_sensors.getOrientation();
		
		sendCommandSequence = SendCommandEnum.SEND;
		performSendCommand(RobotAction.Jog, m_idealOrientation);
		m_commander.reset();
	}

	public void testJog(int jogCycle) {
		int cycle = 0;
		jogSequence = JogEnum.START;
		mainSequence = RunEnum.PERFORM_JOG;
		while(cycle < jogCycle) {
			performJog();

			if (jogSequence == JogEnum.ERROR)
				break;
			
			if (mainSequence != RunEnum.PERFORM_JOG)
				break;
			
			if (jogSequence == JogEnum.DETECT_TREASURE)
				cycle++;
		}
	}
	
	private void testSendCommand(RobotAction action) {
		sendCommandSequence = SendCommandEnum.SEND;
		while(true) {
			if (performSendCommand(action) == 1)
				break;
			
			if (sendCommandSequence == SendCommandEnum.ERROR)
				break;
		}
	}
	
	private void testSendCommand(RobotAction action, int param) {
		sendCommandSequence = SendCommandEnum.SEND;
		while(true) {
			if (performSendCommand(action, param) == 1)
				break;
			
			if (sendCommandSequence == SendCommandEnum.ERROR)
				break;
		}
	}

	public void testStop() {
		RobotPosition robotPosition = m_positionDetector.getPosition();
		String msg = String.format("Location[%s]", robotPosition.location.name());
		log(msg, 1);
		
		sendCommandSequence = SendCommandEnum.SEND;
		performSendCommand(RobotAction.Stop);
	}

	public void testJog2() {
		sleep(MOTION_SETTLING_TIME);
		m_idealOrientation = (int)m_sensors.getOrientation();
		
		sendJog2Sequence = SendJog2Enum.SEND;
		while(true) {
			if (performSendJog2Command() == 1)
				break;
			
			if (sendJog2Sequence == SendJog2Enum.ERROR)
				break;
		}
	}

	public void testArmDown() {
		testSendCommand(RobotAction.ArmDown);
	}
	
	public void testPick() {
		testSendCommand(RobotAction.PickTreasure);
	}

	public void testMove(int distance) {
		testSendCommand(RobotAction.Move, distance);
	}

	public void testTurn(int angle) {
		
		m_curOrientation = (int)m_sensors.getOrientation();
		sleep(MOTION_SETTLING_TIME);
		
		testSendCommand(RobotAction.TurnDegree, angle);

		sleep(MOTION_SETTLING_TIME);
		determineCompensationAngleAfterTurn(angle);
		
		String msg = String.format("Previous[%d], Compensate[%d], Current[%d]", m_curOrientation, m_compensationAngle, (int)m_sensors.getOrientation());
		log(msg, 1);
		
		if (m_compensationAngle != 0) {
			sendCommandSequence = SendCommandEnum.SEND;
			testSendCommand(RobotAction.TurnDegree, m_compensationAngle);
		}
	}
}
